typedef struct
{
	float Kp;
	float Ki;
	float Kd;
	int Tp;

	float error;
	float integral;
	float lastError;
	float derivative;

	int target;
	int currentValue;
	int offset;
	int power;
} PIDControlledMotor;

void runPIDController(PIDControlledMotor &controller)
{
	controller.error=0;
	controller.integral=0;
	controller.lastError=0;
	controller.derivative=0;
	controller.offset=0;
	controller.power=0;
	while(true)
	{
		controller.error = controller.currentValue - controller.target;
		controller.integral = controller.integral + controller.error;
		controller.derivative = controller.error - controller.lastError;
		controller.offset = controller.Kp * controller.error + controller.Ki * controller.integral + controller.Kd * controller.derivative;
		controller.power = controller.Tp + controller.offset;
		controller.lastError = controller.error;
	}
}
